package com.robotic.udp;

import org.springframework.stereotype.Component;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

/**
 * 通过UDP协议切换绝影X30 Pro的速度指令输入源。
 */
@Component
public class SpeedSourceSwitcher {

    // 感知主机的IP地址和端口
    private static final String PERCEPTION_HOST_IP = "10.110.7.219";
    private static final int PERCEPTION_HOST_PORT = 43899;

    private static final int MOTION_HOST_PORT = 43893;
    // “切换速度输入源”的指令码
    private static final int CMD_SET_SPEED_SOURCE = 0x3101EE03;

    // “开始运动/停止运动”的指令码 (用于在踏步和站立之间切换)
    private static final int CMD_TOGGLE_MOTION = 0x31010201;

    /**
     * 定义速度输入源的枚举类型，使代码更清晰、安全。
     */
    public enum SpeedSource {
        /**
         * 对应辅助模式，机器人响应手柄摇杆的指令，并进行避障辅助 [cite: 862, 869]。
         */
        JOYSTICK(1),

        /**
         * 对应导航模式，机器人响应自主导航模块规划的速度指令 [cite: 864, 869]。
         */
        NAVIGATION_MODULE(2);

        private final int value;

        SpeedSource(int value) {
            this.value = value;
        }

        public int getValue() {
            return this.value;
        }
    }

    /**
     * 设置速度输入源。
     * @param source 要设置的速度源（JOYSTICK 或 NAVIGATION_MODULE）。
     * @throws IOException 如果发生网络错误。
     */
    public void setSpeedSource(SpeedSource source) {
        try {
            System.out.println("正在切换速度输入源到: " + source.name());

            try (DatagramSocket socket = new DatagramSocket()) {
                int value = source.getValue();
                if (value == 2) {
                    stopStepping(1);
                }

                InetAddress address = InetAddress.getByName(PERCEPTION_HOST_IP);

                // 根据文档，简单指令是一个12字节的结构体，采用小端字节序
                ByteBuffer buffer = ByteBuffer.allocate(12);
                buffer.order(ByteOrder.LITTLE_ENDIAN);

                // 填充指令结构体
                buffer.putInt(CMD_SET_SPEED_SOURCE); // 指令码 code
                buffer.putInt(value);    // 指令值 value (对应协议的paramters_size字段)
                buffer.putInt(0);                    // 指令类型 type (简单指令为0) [cite: 494]

                byte[] payload = buffer.array();

                // 创建并发送UDP数据包
                DatagramPacket packet = new DatagramPacket(payload, payload.length, address, PERCEPTION_HOST_PORT);
                socket.send(packet);

                if (value == 1) {
                    stopStepping(0);
                }

                System.out.println("成功发送指令到 " + PERCEPTION_HOST_IP + ":" + PERCEPTION_HOST_PORT);
            }
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 发送“开始运动/停止运动”指令。
     * 该指令用于在“力控站立状态”和“踏步状态”之间进行切换。
     * 调用此方法前，应确保机器人处于其中一种状态。 [cite: 2190]
     * 例如，若机器人正在踏步，调用此方法将使其停止踏步并站立。
     * @throws RuntimeException 如果发生网络错误。
     */
    public void stopStepping(int value) {
        try {
            System.out.println("正在发送'停止踏步'指令...");

            try (DatagramSocket socket = new DatagramSocket()) {
                InetAddress address = InetAddress.getByName(PERCEPTION_HOST_IP);

                // 根据文档，简单指令是一个12字节的结构体，采用小端字节序 [cite: 2091, 2087]
                ByteBuffer buffer = ByteBuffer.allocate(12);
                buffer.order(ByteOrder.LITTLE_ENDIAN);

                // 填充指令结构体
                buffer.putInt(CMD_TOGGLE_MOTION); // 指令码 code [cite: 2102]
                buffer.putInt(value);                 // 指令值 value [cite: 2103]
                buffer.putInt(0);                 // 指令类型 type (简单指令为0) [cite: 2105]

                byte[] payload = buffer.array();

                // 创建并发送UDP数据包到运动主机
                DatagramPacket packet = new DatagramPacket(payload, payload.length, address, MOTION_HOST_PORT);
                socket.send(packet);

                System.out.println("成功发送指令到 " + PERCEPTION_HOST_IP + ":" + MOTION_HOST_PORT);
            }
        } catch (IOException e) {
            throw new RuntimeException("发送'停止踏步'指令失败", e);
        }
    }
}
